
from math import *

def didCollide(shipA, shipB):
    delta = hypot(shipA.x-shipB.x, shipA.y-shipB.y);
    return (delta < (shipA.radius+shipB.radius))

def getMinTime(shipA, shipB):
    #define parameters for minimum distance function
    a1 = shipA.u
    b1 = shipA.x
    c1 = shipA.v
    d1 = shipA.y
    a2 = shipB.u
    b2 = shipB.x
    c2 = shipB.v
    d2 = shipB.y
    
    top = - (a1*b1 - a1*b2 - b1*a2 +a2*b2 + c1*d1 - c1*d2 - d1*c2 + c2*d2)
    bot = (a1**2 - 2*a1*a2 + a2**2 + c1**2 - 2*c1*c2 + c2**2)
    
    if (bot == 0.0):
        return 1000000.0
    else:
        return (top/bot) 

def distance(shipA, shipB, t):
    return hypot(shipA.x+shipA.u*t - shipB.x-shipB.u*t,
                 shipA.y+shipA.v*t - shipB.y-shipB.v*t)

def limittheta(ship):
    if (ship.theta >= 2*pi) or (ship.theta <= 0):
        ship.theta = ship.theta % (2*pi)
    return

def step(ship, dt):
    ship.x += ship.u*dt
    ship.y += ship.v*dt
    ship.theta += ship.omega*dt
    ship.u += ship.accel*cos(ship.theta)*dt
    ship.v += ship.accel*sin(ship.theta)*dt
    ship.omega += ship.alpha*dt
    return

def integratePaths(shipA, shipB, dt):
    #initialize times
    n = 256
    epst = dt/n
    
    #initialize rmin
    rmin = distance(shipA, shipB, 0.0)
    
    for i in xrange(n):
        
        #check if collision happens in this timestep
        tmin = getMinTime(shipA, shipB)
        if (tmin >= 0.0) and (tmin <= epst):
            rtemp = distance(shipA, shipB, tmin)
        
        #keep theta from exploding
        limittheta(shipA)
        limittheta(shipB)
        
        #update state
        step(shipA, epst)
        step(shipB, epst)
        
        
        #If predicted time of minimum separation happens
        #in this time step, compare that to rmin        
        if (tmin > 0.0) and (tmin < epst) and (rtemp < rmin):
            rmin = rtemp
        else:
            rtemp = distance(shipA, shipB, 0.0)
            rmin = min(rmin, rtemp)
    return rmin < (shipA.radius+shipB.radius)
